#include "Align.h"

#include <cmath>
#include <stdlib.h>
#include <iostream>

Align::Align(){
  MaxAngularAcceleration = 1000;
//MaxAngularAcceleration = 0.01;
  MaxRotation = 0.005;
  targetRadius = 0.01;
  slowRadius = M_PI/4;        // Esto es 45 grados.
  timeToTarget = 0.01;
}

Steering* Align::getSteering(){
//    cout << "getSteering de Align" << endl;
  Steering* ret = new Steering();
  float targetRotation;
  float rotation =target->orientation - character->orientation;

  //DEBUG
  // cout << " to: " << target->orientation << "co: " << character->orientation << "rotation(dif): " << rotation << endl;
//    cout << "target->orientation " << target->orientation << endl;
//    cout << "character->orientation " << character->orientation << endl;
  mapToRange(rotation);

  // DEBUG
  // cout << " to: " << target->orientation << "co: " << character->orientation << " mapRotation(dif): " << rotation << endl;
//    cout << "rotation" << rotation<< endl;

  float rotationSize = abs(rotation);
//    cout << "rotationSize " << rotationSize << endl;
  if (rotationSize < targetRadius)
    return ret;
  else if ( rotationSize > slowRadius )
    targetRotation= MaxRotation;
  else
    targetRotation = MaxRotation * rotationSize / slowRadius;
//    cout << "Paso gS-A" << endl;
  targetRotation *= rotation / rotationSize;
  ret->angular = targetRotation - character->rotation;
  ret->angular /= timeToTarget;
  float angularAcceleration = abs(ret->angular);
  if (angularAcceleration > MaxAngularAcceleration){
    ret->angular /= angularAcceleration;
    ret->angular *= MaxAngularAcceleration;
  }
  return ret;
}

void Align::mapToRange(float &r){
  if (r > M_PI){
    r -= 2*M_PI;
    // mapToRange(r);
  }else if (r < -M_PI){
    r += 2* M_PI;
    // mapToRange(r);
  }
  /*
  if (abs(target->orientation - character->orientation)<= abs(character->orientation - target->orientation)){
//      cout << " maptoRange() " <<target->orientation - character->orientation << endl;
    return target->orientation - character->orientation;
  }
//    cout << " maptoRange() " <<character->orientation - target->orientation << endl;
  return character->orientation - target->orientation;
  */
}
